/**
* Misc code. 
*  - Does random bits of math I couldn't find in Graphite.
*  - Does init() handling, deconstructor, etc
*/


#include <OGF/quick_start/common/deform.h>
#include <OGF/basic/os/stopwatch.h>
#include <OGF/cells/map/map_cell_heap.h>

#include <set>
#include <stack>
#include <queue>
#include <limits>

#include <OGF/math/linear_algebra/vector.h>

namespace OGF 
{
	
	Deform deform;
	
	/**
	* Build a rotation matrix from an axis-angle rotation
	*/
	Matrix2d Deform::fromAxisAngle(Vector3d axis, double angle)
	{
		double fCos = cos(angle);
		double fSin = sin(angle);
		double fOneMinusCos = 1.0f-fCos;
		double fX2 = axis[0]*axis[0];
		double fY2 = axis[1]*axis[1];
		double fZ2 = axis[2]*axis[2];
		double fXYM = axis[0]*axis[1]*fOneMinusCos;
		double fXZM = axis[0]*axis[2]*fOneMinusCos;
		double fYZM = axis[1]*axis[2]*fOneMinusCos;
		double fXSin = axis[0]*fSin;
		double fYSin = axis[1]*fSin;
		double fZSin = axis[2]*fSin;
	    
		Matrix2d m;
		m(0,0)= fX2*fOneMinusCos+fCos;
		m(0,1) = fXYM-fZSin;
		m(0,2) = fXZM+fYSin;
		m(1,0) = fXYM+fZSin;
		m(1,1) = fY2*fOneMinusCos+fCos;
		m(1,2) = fYZM-fXSin;
		m(2,0) = fXZM-fYSin;
		m(2,1) = fYZM+fXSin;
		m(2,2) = fZ2*fOneMinusCos+fCos;
		return m;
	}

	/**
	* 3x3 matrix - vector multiplication. 
	* Why was this not in Graphtie somewhere already?
	*/
	Vector3d Deform::mult(const Matrix2d &m, const Vector3d &v) const
	{
		double x = m(0,0)*v[0] + m(0,1)*v[1] + m(0,2)*v[2];
		double y = m(1,0)*v[0] + m(1,1)*v[1] + m(1,2)*v[2];
		double z = m(2,0)*v[0] + m(2,1)*v[1] + m(2,2)*v[2];

		Vector3d result(x,y,z);

		return result ;
	}

	Deform::Deform()
	{
		anchor = nil;
		border = nil;
		deltas = nil;
		index = nil;
		solver = nil;
		true_deltas = nil;
		surface = nil;

		//Paramaters:
		gravity = 0.0;
		constraint_weight = 1.0;
		GRAVITY_COORD = 1; //Gravity goes in the 'Y' direction
	}
	Deform::~Deform()
	{
		delete border;
		delete true_deltas;
		delete deltas;
		delete solver;
	}

	/**
	* Do init, but only if the surface has changed.
	* Checks is the surface has changed by pointer equality,
	* and number of verts and half-edges.
	*/
	void Deform::lazy_init(Surface *surface)
	{	
		//Properties we watch on the mesh:
		static int num_verts = -1;
		static int num_edge = -1;

		//if the surface has changed since the last time we ran init(),
		//then run it again.
		if(	surface != this->surface 
			|| num_verts != surface->size_of_vertices() 
			|| num_edge != surface->size_of_halfedges())
		{
			num_verts = surface->size_of_vertices();
			num_edge = surface->size_of_halfedges();

			init(surface); 
		}
	}

	/**
	* Initialize the deformation strucutres.
	* Basically, bind all the attributes.
	*/
	void Deform::init(Surface *surface)
	{
		std::cout<<"Initializing deformation code"<<std::endl;

		this->surface = surface;
		this->anchor = nil;

		constraint_pos = new MapVertexAttribute<Point3d>();
		constraint_pos->bind(surface, "constraint_pos");

		classify = new MapVertexAttribute<CLASS>();
		classify->bind(surface, "classify");

		border = new MapFacetAttribute<bool>();
		border->bind(surface, "roi_border");

		true_deltas = new MapVertexAttribute<Vector3d>();
		true_deltas->bind(surface, "true_deltas");

		deltas = new MapVertexAttribute<Vector3d>();
		deltas->bind(surface, "deltas");
		
		laplace_weight = new MapVertexAttribute<double>;
		laplace_weight->bind(surface, "laplace_weight");

		index = new MapVertexAttribute<int>(surface, "index");

		mass = new MapVertexAttribute<double>;
		mass->bind(surface, "mass");
		
		area = new MapFacetAttribute<double>;
		area->bind(surface, "area");

		true_pos = new MapVertexAttribute<Point3d>;		//original positions
		true_pos->bind(surface, "true_pos");

		//Compute the properties depended only on the original surface
		bake_deformation();		//Bake the un-deformed mesh

		//TODO: put this into the 'bake' code?
		calc_areas();			//find areas of all the faces
		calc_Laplace_weights();
		calc_masses();
	}
} //namespace